#include "line_detection.hpp"
#include <opencv2/opencv.hpp>
#include <vector>
#include <cmath>
#include <iostream>

std::vector<cv::Point2f> sampleLinePoints(const cv::Point2f& p1, const cv::Point2f& p2, int n) {
    std::vector<cv::Point2f> pts;
    for (int i = 0; i <= n; ++i) {
        float alpha = float(i) / n;
        pts.emplace_back((1 - alpha) * p1 + alpha * p2);
    }
    return pts;
}

std::vector<cv::Point2f> extractMultiStraightLinePoints(const std::vector<cv::Point2f>& points, float distanceThreshold, int minPointsPerLine) {
    std::vector<cv::Point2f> remainingPoints = points;
    std::vector<cv::Point2f> allInliers;

    while (remainingPoints.size() >= minPointsPerLine) {
        cv::Vec4f line;
        cv::fitLine(remainingPoints, line, cv::DIST_L2, 0, 0.01, 0.01);

        float vx = line[0], vy = line[1], x0 = line[2], y0 = line[3];
        float a = -vy, b = vx, c = -(a * x0 + b * y0);

        std::vector<cv::Point2f> currentInliers;
        std::vector<cv::Point2f> outliers;

        for (const auto& p : remainingPoints) {
            float d = std::abs(a * p.x + b * p.y + c) / std::sqrt(a * a + b * b);
            if (d < distanceThreshold) {
                currentInliers.push_back(p);
            } else {
                outliers.push_back(p);
            }
        }

        if (currentInliers.size() < minPointsPerLine) {
            break;
        }

        allInliers.insert(allInliers.end(), currentInliers.begin(), currentInliers.end());
        remainingPoints = outliers;

        std::cout << "检测到一组直线，内点数量: " << currentInliers.size() << ", 剩余点: " << remainingPoints.size() << std::endl;
    }

    return allInliers;
}
